% -------------------------------------------------------------------------
% calculate switching function
%
% [Sw , A , B] = Sw_(y , auxdata)
%
% Input argumuents:
% -------------------------------------------------------------------------
% y           [14x1 double]     state and costate variables     [ - ]
% auxdata     [structure]       parameters                      [ - ]
%
% Output argumuents:
% -------------------------------------------------------------------------
% Sw          [1x1 double]      switching function              [ - ]
% A           [6x1 double]      A array                         [ - ]
% B           [6x3 double]      B matrix                        [ - ]
%
% External functions called:
% -------------------------------------------------------------------------
% None
%
% Copyright(C) 2015/06/22 by Chen Zhang,
% School of Astronautics, Beihang University
% chenzhang.buaa@gmail.com
% -------------------------------------------------------------------------
function [Sw , A , B] = Sw_(y , auxdata)
mu = auxdata.mu;
Tmax = auxdata.Tmax;
c = auxdata.c;
p = y(1); ex = y(2); ey = y(3); hx = y(4); hy = y(5); L = y(6); m = y(7);
pp = y(8); pex = y(9); pey = y(10); phx = y(11); phy = y(12); pL = y(13); 
pm = y(14);
W = 1 + ex * cos(L) + ey * sin(L);
Z = hx * sin(L) - hy * cos(L);
C = 1 + hx^2 + hy^2;
Ckep = sqrt(p / mu) / W;
A = Ckep * [0 ; 0 ; 0 ; 0 ; 0 ; W^3 * mu / p^2];
B = Ckep *  [0 , 2*p , 0;
    W * sin(L) , (W + 1) * cos(L) + ex , -Z * ey;
    -W * cos(L) , (W + 1) * sin(L) + ey, Z * ex;
    0 , 0 , C * cos(L) / 2;
    0 , 0 , C * sin(L) / 2;
    0 , 0 , Z];
% calculate | lambda' * B |
LtB = [pp , pex , pey , phx , phy , pL] * B;
LtB_mag = sqrt(LtB(1)^2 + LtB(2)^2 + LtB(3)^2);
% calculate switching function
Sw = -Tmax * ( LtB_mag / m + pm / c);
end
% -------------------------------------------------------------------------
